#pragma once
#include "PinholeCameraC.h"
#include "inc.h"
#include "CalObservation2dC.h"
class WandCalBodyC
{
public:
WandCalBodyC(void);

	WandCalBodyC(PinholeArrayC& _camera, 
		UIntT _frames, 
		bool opt_focal,
		UIntT _dynamic_iterations,
		RealT _max_init_rms);

public:
	RealT NewObs(const SArray1dC<CalObservation2dC>& obs0,
		const SArray1dC<CalObservation2dC>& obs1);

	bool BatchUpdate(UIntT iterations);

	RealT RMSReprojectionError();

	void Scale(RealT s);

	RealT UpdateErrorRMSBest()
	{ return error_rms_best; }

	RealT UpdateErrorRMSCurrent()
	{ return error_rms_current; }

	RealT UpdateErrorPixelMax()
	{ return error_pixel_max; }
     
    void disply(ofstream& out)
	{for (UIntT icam = 0; icam < ncameras; icam++)
			camera[icam].display(cout);	
	}

	void disply(ostream& out)
	{for (UIntT icam = 0; icam < ncameras; icam++)
			camera[icam].display(cout);
	}
public:
	UIntT NumPoints() 
	{ return npoints; }

	const CalObservation2dC& GetObs(UIntT icam, UIntT ipoint)
	{ return obs[icam][ipoint]; }

	UIntT TotalObsAdded() const
	{ return total_frames; }

	~WandCalBodyC(void);
	
protected:
	bool InitGeometry();
	bool LMUpdate();

protected:
	UIntT ncameras;
	UIntT nframes;
	UIntT npoints;
	UIntT nparams_cam;
	UIntT nparams_point;
	UIntT nparams_point_constraint;
	UIntT nobs;
	bool opt_focal;
	RealT wandlength;
	PinholeArrayC camera;
	bool have_init_geometry;
	UIntT add_index;
	UIntT total_frames;
	SArray1dC< CalObservation2dArrayC > obs;
	SArray1dC< Vector2dC > focal0;
	RealT max_init_rms;
	UIntT dynamic_iterations;

	// current error values
	RealT error_rms_current;
	RealT error_rms_best;
	RealT error_pixel_max;

	// LM best guess state params
	VectorC xa0;
	VectorC xb0;
	RealT lambda;
};
